#include <ros/ros.h>
#include <can_msgs/Frame.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/Float64MultiArray.h>
#include <robot_solver_pkg/ArmControl.h>
#include <mutex>
#include <vector>

#include "serial_arm.h"

class MiddleCodingNode
{
private:
    ros::NodeHandle nh_;

    // 订阅者
    ros::Subscriber canmsg_up_sub_;

    // 发布者
    ros::Publisher canmsg_down_pub_;

    // 服务客户端
    ros::ServiceClient arm_control_client_;

    // 数据存储
    robot_solver_pkg::ArmControl srv;
    bool joints_flag1, joints_flag2, pose_flag;

    void canmsgUpCallback(const can_msgs::Frame::ConstPtr &msg)
    {
        int8_t datas[8];

        for (int i = 0; i < 8; i++)
            datas[i] = msg->data[i];

        // 检查frame_id
        if (msg->id == 6) // 目前关节角度数据
        {
            SerialArm_joints joints;
            SerialArm_joints_decode(datas, &joints);
            if (joints.state == 0)
            {
                srv.request.current_joints[0] = joints.joint0_or_reserved;
                srv.request.current_joints[1] = joints.joint14;
                srv.request.current_joints[2] = joints.joint25;
                srv.request.current_joints[3] = joints.joint36;
                joints_flag1 = true;
            }
            else if (joints.state == 1)
            {
                srv.request.current_joints[4] = joints.joint14;
                srv.request.current_joints[5] = joints.joint25;
                srv.request.current_joints[6] = joints.joint36;
                joints_flag2 = true;
            }
        }
        else if (msg->id == 7)
        {
            SerialArm_pose pose;
            SerialArm_pose_decode(datas, &pose);
            srv.request.pose_delta.linear.x = pose.x;
            srv.request.pose_delta.linear.y = pose.y;
            srv.request.pose_delta.linear.z = pose.z;
            srv.request.pose_delta.angular.x = pose.roll;
            srv.request.pose_delta.angular.y = pose.pitch;
            srv.request.pose_delta.angular.z = pose.yaw;
            pose_flag = true;
        }
        else
            return;

        // 尝试解算
        if (joints_flag1 && joints_flag2 && pose_flag)
        {
            joints_flag1 = joints_flag2 = pose_flag = false;
            callArmControlService();
        }
    }

    void callArmControlService()
    {
        // 调用服务
        if (arm_control_client_.call(srv))
        {
            ROS_INFO("Service call successful. Sending joint angles to CAN.");

            std::vector<double> joint_angles_list;
            for (const auto &joint_angle : srv.response.joint_angles)
            joint_angles_list.push_back(joint_angle);

            sendJointAnglesToCAN(joint_angles_list);
        }
        else
        {
            ROS_ERROR("Failed to call arm_control_service.");
        }
    }

    void sendJointAnglesToCAN(const std::vector<double> &joint_angles)
    {
        // 将关节角数据编码为CAN消息
        can_msgs::Frame can_msg;
        SerialArm_joints arm_joints;
        int8_t data_formated[8];

        can_msg.id = 0x004;
        can_msg.is_extended = false;
        can_msg.is_error = false;
        can_msg.dlc = 8;

        // 关节数据比较长，所以分两端发送
        /// 发送第一段
        arm_joints.state = 0;
        arm_joints.joint0_or_reserved = joint_angles[0];
        arm_joints.joint14 = joint_angles[1];
        arm_joints.joint25 = joint_angles[2];
        arm_joints.joint36 = joint_angles[3];
        SerialArm_joints_encode(arm_joints, data_formated);

        for (int i = 0; i < 8; i++)
            can_msg.data[i] = data_formated[i];

        canmsg_down_pub_.publish(can_msg);

        /// 发送第二段
        can_msg.id = 0x005;
        arm_joints.state = 1;
        arm_joints.joint0_or_reserved = 0;
        arm_joints.joint14 = joint_angles[4];
        arm_joints.joint25 = joint_angles[5];
        arm_joints.joint36 = joint_angles[6];
        SerialArm_joints_encode(arm_joints, data_formated);

        for (int i = 0; i < 8; i++)
            can_msg.data[i] = data_formated[i];
        canmsg_down_pub_.publish(can_msg);

        ROS_INFO("Joint angle data sent to CAN.");
    }

public:
    MiddleCodingNode()
    {
        // 初始化订阅者
        canmsg_up_sub_ = nh_.subscribe("can_upward_topic", 10,
                                       &MiddleCodingNode::canmsgUpCallback, this);

        // 初始化发布者
        canmsg_down_pub_ = nh_.advertise<can_msgs::Frame>(
            "can_downward_topic", 10);

        // 初始化服务客户端
        arm_control_client_ = nh_.serviceClient<robot_solver_pkg::ArmControl>("arm_control_service");

        // 初始化异步存储
        joints_flag1 = false;
        joints_flag2 = false;
        pose_flag = false;
    }
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "middle_coding_node");
    MiddleCodingNode node;
    ros::spin();
    return 0;
}